// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------
// Modification history:
// feature: Code style modification
// ------------------------------------------------------------------

namespace evbs {
namespace ertps {
namespace rtps {

bool RTPSMessageCreator::addMessageHeartbeat(CDRMessage_t* msg, const GuidPrefix_t& guidprefix,
                                             const EntityId_t& readerId, const EntityId_t& writerId,
                                             const SequenceNumber_t& firstSN, const SequenceNumber_t& lastSN,
                                             Count_t count, bool isFinal, bool livelinessFlag) {
    (void)RTPSMessageCreator::addHeader(msg, guidprefix);
    (void)RTPSMessageCreator::addSubmessageHeartbeat(msg, readerId, writerId, firstSN, lastSN, count, isFinal,
                                                     livelinessFlag);
    msg->length = msg->pos;
    return true;
}

bool RTPSMessageCreator::addMessageHeartbeat(CDRMessage_t* msg, const GuidPrefix_t& guidprefix,
                                             const GuidPrefix_t& remoteGuidprefix, const EntityId_t& readerId,
                                             const EntityId_t& writerId, const SequenceNumber_t& firstSN,
                                             const SequenceNumber_t& lastSN, Count_t count, bool isFinal,
                                             bool livelinessFlag) {
    (void)RTPSMessageCreator::addHeader(msg, guidprefix);
    (void)RTPSMessageCreator::addSubmessageInfoDST(msg, remoteGuidprefix);
    (void)RTPSMessageCreator::addSubmessageHeartbeat(msg, readerId, writerId, firstSN, lastSN, count, isFinal,
                                                     livelinessFlag);
    msg->length = msg->pos;
    return true;
}

bool RTPSMessageCreator::addSubmessageHeartbeat(CDRMessage_t* msg, const EntityId_t& readerId,
                                                const EntityId_t& writerId, const SequenceNumber_t& firstSN,
                                                const SequenceNumber_t& lastSN, Count_t count, bool isFinal,
                                                bool livelinessFlag) {
    octet flags = 0x0U;

    Endianness_t old_endianess = msg->msg_endian;
#if EDDS_IS_BIG_ENDIAN_TARGET
    msg->msg_endian = BIGEND;
#else
    flags = flags | BIT(0);
    msg->msg_endian = LITTLEEND;
#endif  // if EDDS_IS_BIG_ENDIAN_TARGET

    if (isFinal) {
        flags = flags | BIT(1);
    }
    if (livelinessFlag) {
        flags = flags | BIT(2);
    }

    // Submessage header.
    (void)CDRMessage::addOctet(msg, HEARTBEAT);
    (void)CDRMessage::addOctet(msg, flags);
    uint32_t submessage_size_pos = msg->pos;
    uint16_t submessage_size = 0U;
    (void)CDRMessage::addUInt16(msg, submessage_size);
    uint32_t position_size_count_size = msg->pos;

    (void)CDRMessage::addEntityId(msg, &readerId);
    (void)CDRMessage::addEntityId(msg, &writerId);
    // Add Sequence Number
    (void)CDRMessage::addSequenceNumber(msg, &firstSN);
    (void)CDRMessage::addSequenceNumber(msg, &lastSN);
    (void)CDRMessage::addInt32(msg, (int32_t)count);

    // TODO(Ricardo) Improve.
    submessage_size = uint16_t(msg->pos - position_size_count_size);
    octet* o = reinterpret_cast<octet*>(&submessage_size);

    if (msg->buffer == nullptr || submessage_size_pos + 1 >= msg->max_size) {
        return false;
    }

    if (msg->msg_endian == DEFAULT_ENDIAN) {
        msg->buffer[submessage_size_pos] = *(o);
        msg->buffer[submessage_size_pos + 1U] = *(o + 1);
    } else {
        msg->buffer[submessage_size_pos] = *(o + 1);
        msg->buffer[submessage_size_pos + 1U] = *(o);
    }

    msg->msg_endian = old_endianess;

    return true;
}

bool RTPSMessageCreator::addMessageHeartbeatFrag(CDRMessage_t* msg, const GuidPrefix_t& guidprefix,
                                                 const EntityId_t& readerId, const EntityId_t& writerId,
                                                 SequenceNumber_t& firstSN, FragmentNumber_t& lastFN, Count_t count) {
    (void)RTPSMessageCreator::addHeader(msg, guidprefix);
    (void)RTPSMessageCreator::addSubmessageHeartbeatFrag(msg, readerId, writerId, firstSN, lastFN, count);
    msg->length = msg->pos;
    return true;
}

bool RTPSMessageCreator::addSubmessageHeartbeatFrag(CDRMessage_t* msg, const EntityId_t& readerId,
                                                    const EntityId_t& writerId, SequenceNumber_t& firstSN,
                                                    FragmentNumber_t& lastFN, Count_t count) {
    octet flags = 0x0U;
    Endianness_t old_endianess = msg->msg_endian;
#if EDDS_IS_BIG_ENDIAN_TARGET
    msg->msg_endian = BIGEND;
#else
    flags = flags | BIT(0);
    msg->msg_endian = LITTLEEND;
#endif  // if EDDS_IS_BIG_ENDIAN_TARGET

    // Submessage header.
    (void)CDRMessage::addOctet(msg, HEARTBEAT_FRAG);
    (void)CDRMessage::addOctet(msg, flags);
    uint32_t submessage_size_pos = msg->pos;
    uint16_t submessage_size = 0U;
    (void)CDRMessage::addUInt16(msg, submessage_size);
    uint32_t position_size_count_size = msg->pos;

    (void)CDRMessage::addEntityId(msg, &readerId);
    (void)CDRMessage::addEntityId(msg, &writerId);
    // Add Sequence Number
    (void)CDRMessage::addSequenceNumber(msg, &firstSN);
    (void)CDRMessage::addUInt32(msg, (uint32_t)lastFN);
    (void)CDRMessage::addInt32(msg, (int32_t)count);

    // TODO(Ricardo) Improve.
    submessage_size = uint16_t(msg->pos - position_size_count_size);
    octet* o = reinterpret_cast<octet*>(&submessage_size);
    if (msg->msg_endian == DEFAULT_ENDIAN) {
        msg->buffer[submessage_size_pos] = *(o);
        msg->buffer[submessage_size_pos + 1U] = *(o + 1);
    } else {
        msg->buffer[submessage_size_pos] = *(o + 1);
        msg->buffer[submessage_size_pos + 1U] = *(o);
    }

    msg->msg_endian = old_endianess;

    return true;
}

}  // namespace rtps
}  // namespace ertps
}  // namespace evbs
